/********************************************************************
Crytek Source File.
Copyright (C), Crytek Studios, 2001-2011.
-------------------------------------------------------------------------
File name:   PatHolder.h
$Id$
Description: Interface to hold an manipulate a path

-------------------------------------------------------------------------
History:
- 30/11/2011 : Created by Francesco Roccucci

*********************************************************************/

#ifndef __PathHolder_h__
#define __PathHolder_h__

#pragma once

#include "NavPath.h"
#include "INavigationSystem.h"

template<class T>
class CPathHolder
{
public:
	typedef std::vector<T> PathHolderPath;

	CPathHolder()
	{
		Reset();
	}

	void PushBack(const T& element)
	{
		m_path.push_back(element);
	}

	void PushFront(const T& element)
	{
		m_path.insert(m_path.begin(),element);
	}

	void Reset()
	{
		m_path.clear();
	}

	const PathHolderPath& GetPath() const { return m_path; }
	inline typename PathHolderPath::size_type Size() const { return m_path.size(); }

	void PullPathOnNavigationMesh(const NavigationMeshID meshID, uint16 iteration, MNM::WayTriangleData* way, const size_t maxLenght)
	{
		FUNCTION_PROFILER(GetISystem(), PROFILE_AI);

		/*
			MNM::WayTriangleData* way is a pointer to an array of maxLenght
			elements. It's important to remember that the FindWay returns
			that array filled in the opposite order (the first triangle
			is the one in the last position)
		*/
		CTimeValue startingTime = gEnv->pTimer->GetAsyncTime();

		if(m_path.size() < 3)
			return; 

		const NavigationMesh& mesh = gAIEnv.pNavigationSystem->GetMesh(meshID);
		const MNM::MeshGrid& grid = mesh.grid;

		for(uint16 i=0; i < iteration; ++i)
		{
			typename PathHolderPath::reverse_iterator it = m_path.rbegin();
			typename PathHolderPath::reverse_iterator end = m_path.rend();

			// Each triplets is associated with two triangles so the way lenght
			// should match the iteration through the path points
			size_t currentPosition = 0;
			for(;it+2 != end && currentPosition < maxLenght; ++it)
			{
				const T& startPoint = *it;
				T& middlePoint = *(it+1);
				const T& endPoint = *(it+2);
				// Let's pull only the triplets that are fully not interacting with SO
				if( middlePoint.navType != IAISystem::NAV_SMARTOBJECT &&
					endPoint.navType != IAISystem::NAV_SMARTOBJECT)
				{

					const Vec3& from = startPoint.vPos;
					const Vec3& to = endPoint.vPos;
					Vec3& middle = middlePoint.vPos;

					MNM::vector3_t startLocation, middleLocation, endLocation;
					startLocation.Set(MNM::real_t(from.x), MNM::real_t(from.y), MNM::real_t(from.z));
					middleLocation.Set(MNM::real_t(middle.x), MNM::real_t(middle.y), MNM::real_t(middle.z));		
					endLocation.Set(MNM::real_t(to.x), MNM::real_t(to.y), MNM::real_t(to.z));		

					grid.PullString(startLocation, way[currentPosition].triangleID, endLocation, way[currentPosition+1].triangleID, middleLocation);
					middle = middleLocation.GetVec3();
				}

				// On the exit the smart object the middle point of the triangle
				// is added in the path, so when the middle point is a smart object
				// it means we are still in the previous triangle (the way is from the end to the start)
				if(middlePoint.navType != IAISystem::NAV_SMARTOBJECT)
					++currentPosition;
			}
		}
	}

	void FillNavPath(CNavPath& navPath)
	{
		typename PathHolderPath::const_iterator it = m_path.begin();
		typename PathHolderPath::const_iterator end = m_path.end();
		for(; it != end; ++it)
		{
			navPath.PushBack(*it);
		}
	}

	Vec3 At(typename PathHolderPath::size_type pathPosition) const
	{
		Vec3 pos(ZERO);
		if(pathPosition < m_path.size())
		{
			pos = m_path[pathPosition].vPos;
		}
		else
		{
			assert(0);
		}
		return pos;
	}

private:
	PathHolderPath m_path;
};

#endif // __PathHolder_h__